#include <stdio.h>
#include <stdlib.h>
#include <dos.h>
#include <math.h>
#include <conio.h>
#include "defs.h"
#include "kernel.h"

extern int version;

void video_write(BYTE, BYTE, double, int, int);
int move_machine(struct hard_setup *, struct soft_setup *, struct pos *,
                 float *, float *, float, double, double, double, double, double);
double get_unit_vect(struct unit_vect *, double, double, double, double, double, int);
float max_speed_sq(struct hard_setup *,
                   float, float, float, float, float, 
                   struct unit_vect *);
float max_speed_sq_line(struct hard_setup *, double, double, double, double, double);
int backlash(struct hard_setup *, struct soft_setup *, struct unit_vect*, struct unit_vect*);
int backlash_to_come(struct hard_setup *, struct unit_vect *, struct unit_vect *, int, int);
float get_max_speed(struct hard_setup *, float, float, float, float, float);
float get_max_accel(struct hard_setup *, float, float, float, float, float);
float get_min_speed(struct hard_setup *, float, float, float, float, float);
float get_max_4(float, float, float, float);
int sign(float);
double total_vect(double, double, double, double);
void _interrupt _far new_direct_timer(void);
void set_timer_freq(unsigned long);
void _interrupt _far MyKeyboardRoutine(void);
void init_key(void);
void restore_key(void);

//hardware bit manipulation functions
void clear_data_bits(struct hard_setup *hard_config)
{
   outportb((*hard_config).port, 0);
}

void set_io_bit(struct hard_setup *hard_config, int bit)
{
   outportb((*hard_config).port+2, ((inportb((*hard_config).port+2)^11)|bit)^11);
}

void clear_io_bit(struct hard_setup *hard_config, int bit)
{
   BYTE iobits;

   iobits=(inportb((*hard_config).port+2)^11)|bit;
   outportb((*hard_config).port+2, (iobits&(iobits-bit))^11);
}

void toggle_io_bit(struct hard_setup *hard_config, int bit)
{
   bit=pow(2,bit);
   outportb((*hard_config).port+2, ((inportb((*hard_config).port+2)^11)^bit)^11);
}

//update the display
void update_display(struct soft_setup *soft_config, double x, double y, double z,
                    double a, float speed, float a_speed, float f_rate, int item)
{
   if((*soft_config).display_x && (item==0 || item==ALL)) 
      video_write((*soft_config).x_xpos,(*soft_config).x_ypos, x, RED, LONG);
   if((*soft_config).display_y && (item==1 || item==ALL)) 
      video_write((*soft_config).y_xpos,(*soft_config).y_ypos, y, RED, LONG);
   if((*soft_config).display_z && (item==2 || item==ALL)) 
      video_write((*soft_config).z_xpos,(*soft_config).z_ypos, z, RED, LONG);
   if((*soft_config).display_s && (item==3 || item==ALL))
      video_write((*soft_config).s_xpos,(*soft_config).s_ypos, speed*(*soft_config).time, BLACK, SHORT);
   if((*soft_config).display_a && (item==4 || item==ALL)) 
      video_write((*soft_config).a_xpos,(*soft_config).a_ypos, a, RED, LONG);
   if((*soft_config).display_as && (item==5 || item==ALL))
      video_write((*soft_config).as_xpos,(*soft_config).as_ypos, a_speed*(*soft_config).a_time, BLACK, SHORT);
   if((*soft_config).display_fr && (item==6 || item==ALL))
      video_write((*soft_config).fr_xpos,(*soft_config).fr_ypos, f_rate*(*soft_config).time, BLACK, SHORT);
}

void video_write(BYTE x, BYTE y, double dnum, int bkg, int lgth)
{
   BYTE _far *video=(BYTE _far *)0xB8000000L;       /*  Ptr to base of VGA text RAM  */
   BYTE unit;
   WORD yoff=(y-1)*160, xoff=(x-1)*2;
   int s=sign(dnum), i;
   double temp;
   unsigned long lnum=(unsigned long)(temp=s*dnum*(10000)), den=10000000;

   if(temp-lnum>=.5)lnum++;
   if(lgth==SHORT && lnum/100.0-lnum/100>=.5)lnum=lnum+100;
   if(lnum==0) s=1;
   if(lnum>99999999) lnum=0;
   for(i=1; i<17+lgth*4; i+=2) *(video+(WORD)(xoff+i+yoff))=WHITE+bkg*16;
   *(video+(WORD)(xoff+10+yoff))='.';
   if(s<0) *(video+(WORD)(xoff+yoff))='-';
   else *(video+(WORD)(xoff+yoff))=' ';
   for(i=2; i<16+lgth*4; i+=2)
     {if(i==10) i+=2; 
      unit=lnum/den; lnum=lnum-unit*den; den=den/10;
      *(video+(WORD)(xoff+i+yoff))=unit+48;
     }
}

//realtime function for moving device
volatile int tick;
volatile int key, jogging;
int move_machine(struct hard_setup *hard_config, struct soft_setup *soft_config, struct pos *current,
                 float *cur_speed, float *sp_limit, float max_end_speed_sq,
                 double dist_x, double dist_y, double dist_z, double dist_a, double total_vect)
{
   double current_x=0, current_y=0, current_z=0, current_a=0, current_vect=0, temp;
   static double inc_x, inc_y, inc_z, inc_a;
   static float cur_debo_x, cur_debo_y, cur_debo_z, cur_debo_a;
   static float display_count;
   static float cur_x_sp, cur_y_sp, cur_z_sp, cur_a_sp;
   float speed, max_accel, cur_decel, max_speed, min_speed, speed_inc;
   static int x_limit=ON, y_limit=ON, z_limit=ON, a_limit=ON, display;
   static int pxs=1, pys=1, pzs=1, pas=1, x_step=LOW, y_step=LOW, z_step=LOW, a_step=LOW, act_step=HIGH;
   int xs=sign(dist_x), ys=sign(dist_y), zs=sign(dist_z), as=sign(dist_a);
   int msg=COMPLETE, step_flag=OFF, break_flag=OFF, accelerate=NORMAL;
   int acc_x=NORMAL, acc_y=NORMAL, acc_z=NORMAL, acc_a=NORMAL;
   BYTE dir=0, s_port_read, c_port_read;
   static BYTE step;

   //check step activate parameter and change if needed
   if(act_step!=(*hard_config).act_step)
     {act_step=(*hard_config).act_step;
      if(x_step==LOW) x_step=HIGH; else x_step=LOW;
      if(y_step==LOW) y_step=HIGH; else y_step=LOW;
      if(z_step==LOW) z_step=HIGH; else z_step=LOW;
      if(a_step==LOW) a_step=HIGH; else a_step=LOW;
      if((step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit; else step=step-(*hard_config).x_sbit;
      if((step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit; else step=step-(*hard_config).y_sbit;
      if((step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit; else step=step-(*hard_config).z_sbit;
      if((step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit; else step=step-(*hard_config).a_sbit;
      outportb((*hard_config).port, step);
     }

   //set up auxiliary channel for move
   if((*soft_config).a_status==OFF) dist_a=0;
   else if(dist_a==0)
     {if((*soft_config).a_follow==FOLLOW_X) {dist_a=dist_x; as=xs;}
      if((*soft_config).a_follow==FOLLOW_Y) {dist_a=dist_y; as=ys;}
      if((*soft_config).a_follow==FOLLOW_Z) {dist_a=dist_z; as=zs;}
      if((*soft_config).a_follow!=INDEPENDANT)
        {if((*soft_config).units==IN && (*soft_config).a_units==MM) dist_a=dist_a*25.4;
         else if((*soft_config).units==MM && (*soft_config).a_units==IN) dist_a=dist_a/25.4;
        }
     }

   //return if vector is zero length
   if(dist_x==0 && dist_y==0 && dist_z==0 && dist_a==0) return COMPLETE;

   //reset limit switches to original state if limits are turned off
   if((*soft_config).limit==OFF) 
     {x_limit=y_limit=z_limit=a_limit=ON;
      cur_debo_x=cur_debo_y=cur_debo_z=cur_debo_a=0;
     }

   //check for invalid speed parameter
   if(fabs(*sp_limit)<.00001) return INVALID_PARAMETER;
   
   //define directions
   if((*hard_config).x_pdir==HIGH && dist_x>0) dir=dir+(*hard_config).x_dbit;
   if((*hard_config).x_pdir==LOW  && dist_x<0) dir=dir+(*hard_config).x_dbit;
   if((*hard_config).y_pdir==HIGH && dist_y>0) dir=dir+(*hard_config).y_dbit;
   if((*hard_config).y_pdir==LOW  && dist_y<0) dir=dir+(*hard_config).y_dbit;
   if((*hard_config).z_pdir==HIGH && dist_z>0) dir=dir+(*hard_config).z_dbit;
   if((*hard_config).z_pdir==LOW  && dist_z<0) dir=dir+(*hard_config).z_dbit;
   if((*hard_config).a_pdir==HIGH && dist_a>0 && (*soft_config).a_status==ON) dir=dir+(*hard_config).a_dbit;
   if((*hard_config).a_pdir==LOW  && dist_a<0 && (*soft_config).a_status==ON) dir=dir+(*hard_config).a_dbit;
   if((*soft_config).a_status==OFF) dir=dir+(*hard_config).a_dbit;
   
   //set up increment for new direction if sign change
   if(pxs!=xs) {if(act_step==HIGH && (step&(*hard_config).x_sbit)==(*hard_config).x_sbit) step=step-(*hard_config).x_sbit;
                if(act_step==LOW && (step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit;
                if(x_step==HIGH) x_step=LOW; else x_step=HIGH;
                inc_x=(*hard_config).x_per_step/2-inc_x;}
   if(pys!=ys) {if(act_step==HIGH && (step&(*hard_config).y_sbit)==(*hard_config).y_sbit) step=step-(*hard_config).y_sbit;
                if(act_step==LOW && (step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit;
                if(y_step==HIGH) y_step=LOW; else y_step=HIGH;
                inc_y=(*hard_config).y_per_step/2-inc_y;}
   if(pzs!=zs) {if(act_step==HIGH && (step&(*hard_config).z_sbit)==(*hard_config).z_sbit) step=step-(*hard_config).z_sbit;
                if(act_step==LOW && (step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit;
                if(z_step==HIGH) z_step=LOW; else z_step=HIGH;
                inc_z=(*hard_config).z_per_step/2-inc_z;}
   if(pas!=as) {if(act_step==HIGH && (step&(*hard_config).a_sbit)==(*hard_config).a_sbit) step=step-(*hard_config).a_sbit;
                if(act_step==LOW && (step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit;
                if(a_step==HIGH) a_step=LOW; else a_step=HIGH;
                inc_a=(*hard_config).a_per_step/2-inc_a;}
   pxs=xs; pys=ys; pzs=zs; pas=as;
   outportb((*hard_config).port, dir+step);

   if(*sp_limit>0){
   //define speed inc.
   if(dist_x==0 && dist_y==0 && dist_z==0) speed_inc=(*soft_config).a_speed_inc;
   else speed_inc=(*soft_config).speed_inc;

   //define maximum acceleration
   max_accel=get_max_accel(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //define maximum speed
   max_speed=get_max_speed(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //define minimum speed
   min_speed=get_min_speed(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //reset speeds accordingly
   if(*cur_speed<min_speed && *sp_limit>=min_speed) *cur_speed=min_speed;
   if(*cur_speed<*sp_limit && *sp_limit<min_speed) *cur_speed=min_speed=*sp_limit;
   }
   else *cur_speed=0;

   //begin control loop
   while(1)
     {while(tick);
      tick=1;

      //read status and control ports
      if((*soft_config).limit==ON || (*soft_config).e_stop!=OFF) s_port_read=(248&inportb((*hard_config).port+1))^128;
      if((*soft_config).io1_cmm==ON || (*soft_config).io2_cmm==ON ||
         (*soft_config).io3_cmm==ON || (*soft_config).io4_cmm==ON)
         c_port_read=(15&inportb((*hard_config).port+2))^11;
      
      //check for e-stop button or hold line trip
      if((*soft_config).e_stop!=OFF && (*hard_config).e_trip*(*hard_config).e_bit-(s_port_read&(*hard_config).e_bit)==0)
        {if((*soft_config).e_stop==E_STOP_L) {cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0; msg=E_STOP;}
         else while((*hard_config).e_trip*(*hard_config).e_bit-(s_port_read&(*hard_config).e_bit)==0)
           {s_port_read=(248&inportb((*hard_config).port+1))^128;
            //check for ESC key
            if(key==0x01) {key=0; msg=E_STOP; break;}
           }
         if(msg==E_STOP) break;
        }

      if((*soft_config).limit==ON)
        {//check for limit switch trip
         if(x_limit==ON)
           {if((*hard_config).x_ltrip*(*hard_config).x_lbit-(s_port_read&(*hard_config).x_lbit)==0)
              {if(*sp_limit>0) cur_debo_x=cur_debo_x+*cur_speed*(*soft_config).dt*dist_x*xs/total_vect;
               else cur_debo_x=cur_debo_x+cur_x_sp*(*soft_config).dt;
               if(cur_debo_x>(*hard_config).debo_x)
                 {cur_x_sp=0; cur_debo_x=0; x_limit=OFF; if(xs>0) msg=X_PLIMIT; else msg=X_NLIMIT; break;}}
            else cur_debo_x=0;} 
         if(y_limit==ON)
           {if((*hard_config).y_ltrip*(*hard_config).y_lbit-(s_port_read&(*hard_config).y_lbit)==0)
              {if(*sp_limit>0) cur_debo_y=cur_debo_y+*cur_speed*(*soft_config).dt*dist_y*ys/total_vect;
               else cur_debo_y=cur_debo_y+cur_y_sp*(*soft_config).dt;
               if(cur_debo_y>(*hard_config).debo_y)
                 {cur_y_sp=0; cur_debo_y=0; y_limit=OFF; if(ys>0) msg=Y_PLIMIT; else msg=Y_NLIMIT; break;}}
            else cur_debo_y=0;}
         if(z_limit==ON)
           {if((*hard_config).z_ltrip*(*hard_config).z_lbit-(s_port_read&(*hard_config).z_lbit)==0)
              {if(*sp_limit>0) cur_debo_z=cur_debo_z+*cur_speed*(*soft_config).dt*dist_z*zs/total_vect;
               else cur_debo_z=cur_debo_z+cur_z_sp*(*soft_config).dt;
               if(cur_debo_z>(*hard_config).debo_z)
                 {cur_z_sp=0; cur_debo_z=0; z_limit=OFF; if(zs>0) msg=Z_PLIMIT; else msg=Z_NLIMIT; break;}}
            else cur_debo_z=0;}
         if(a_limit==ON && (*soft_config).a_status==ON)
           {if((*hard_config).a_ltrip*(*hard_config).a_lbit-(s_port_read&(*hard_config).a_lbit)==0)
              {if(*sp_limit>0) cur_debo_a=cur_debo_a+*cur_speed*(*soft_config).dt*dist_a*as/total_vect;
               else cur_debo_a=cur_debo_a+cur_a_sp*(*soft_config).dt;
               if(cur_debo_a>(*hard_config).debo_a)
                 {cur_a_sp=0; cur_debo_a=0; a_limit=OFF; if(as>0) msg=A_PLIMIT; else msg=A_NLIMIT; break;}}
            else cur_debo_a=0;}

         //check for limit switch release
         if(x_limit==OFF)
           {if((*hard_config).x_ltrip*(*hard_config).x_lbit-(s_port_read&(*hard_config).x_lbit)!=0)
              {if(*sp_limit>0) cur_debo_x=cur_debo_x+*cur_speed*(*soft_config).dt*dist_x*xs/total_vect;
               else cur_debo_x=cur_debo_x+cur_x_sp*(*soft_config).dt;
               if(cur_debo_x>(*hard_config).debo_x) {cur_debo_x=0; x_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_x=0;}
         if(y_limit==OFF)
           {if((*hard_config).y_ltrip*(*hard_config).y_lbit-(s_port_read&(*hard_config).y_lbit)!=0)
              {if(*sp_limit>0) cur_debo_y=cur_debo_y+*cur_speed*(*soft_config).dt*dist_y*ys/total_vect;
               else cur_debo_y=cur_debo_y+cur_y_sp*(*soft_config).dt;
               if(cur_debo_y>(*hard_config).debo_y) {cur_debo_y=0; y_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_y=0;}
         if(z_limit==OFF)
           {if((*hard_config).z_ltrip*(*hard_config).z_lbit-(s_port_read&(*hard_config).z_lbit)!=0)
              {if(*sp_limit>0) cur_debo_z=cur_debo_z+*cur_speed*(*soft_config).dt*dist_z*zs/total_vect;
               else cur_debo_z=cur_debo_z+cur_z_sp*(*soft_config).dt;
               if(cur_debo_z>(*hard_config).debo_z) {cur_debo_z=0; z_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_z=0;}
         if(a_limit==OFF && (*soft_config).a_status==ON)
           {if((*hard_config).a_ltrip*(*hard_config).a_lbit-(s_port_read&(*hard_config).a_lbit)!=0)
              {if(*sp_limit>0) cur_debo_a=cur_debo_a+*cur_speed*(*soft_config).dt*dist_a*as/total_vect;
               else cur_debo_a=cur_debo_a+cur_a_sp*(*soft_config).dt;
               if(cur_debo_a>(*hard_config).debo_a) {cur_debo_a=0; a_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_a=0;}
        }

      if(*sp_limit>0){
      //set acceleration flag to decel if applicable
      if(accelerate==NORMAL && total_vect>current_vect+.0001)
        {cur_decel=(max_end_speed_sq-*cur_speed**cur_speed)/(2*(total_vect-current_vect));
         if(cur_decel<=-max_accel) accelerate=DECEL;}

      //do acceleration routine and define speed
      if(accelerate==NORMAL)
        {if(*cur_speed<=*sp_limit)
           {*cur_speed=*cur_speed+max_accel*(*soft_config).dt;
            if(*cur_speed>min(*sp_limit, max_speed)) *cur_speed=min(*sp_limit, max_speed);}
         else
           {*cur_speed=*cur_speed-max_accel*(*soft_config).dt;
            if(*cur_speed<min(*sp_limit, max_speed)) *cur_speed=min(*sp_limit, max_speed);}
        }
      if(accelerate==DECEL)
        {*cur_speed=*cur_speed-max_accel*(*soft_config).dt;
         if(*cur_speed<min_speed) *cur_speed=min_speed;}

      //increment position
      current_vect=current_vect+*cur_speed*(*soft_config).dt;
      temp=*cur_speed*(*soft_config).dt/total_vect;
      if(current_x*xs<dist_x*xs) {current_x=current_x+temp*dist_x; inc_x=inc_x+temp*dist_x*xs;}
      if(current_y*ys<dist_y*ys) {current_y=current_y+temp*dist_y; inc_y=inc_y+temp*dist_y*ys;}
      if(current_z*zs<dist_z*zs) {current_z=current_z+temp*dist_z; inc_z=inc_z+temp*dist_z*zs;}
      if(current_a*as<dist_a*as) {current_a=current_a+temp*dist_a; inc_a=inc_a+temp*dist_a*as;}
      if(dist_x*xs<=current_x*xs) {inc_x=inc_x-(current_x*xs-dist_x*xs); current_x=dist_x;}
      if(dist_y*ys<=current_y*ys) {inc_y=inc_y-(current_y*ys-dist_y*ys); current_y=dist_y;}
      if(dist_z*zs<=current_z*zs) {inc_z=inc_z-(current_z*zs-dist_z*zs); current_z=dist_z;}
      if(dist_a*as<=current_a*as) {inc_a=inc_a-(current_a*as-dist_a*as); current_a=dist_a;}
      }
      else{  //if fast non-coordinated move
      //set acceleration flag to decel if applicable
      if(acc_x==NORMAL && dist_x*xs>current_x*xs+.0001)
        {cur_decel=((*hard_config).min_vel_x*(*hard_config).min_vel_x-cur_x_sp*cur_x_sp)/(2*(dist_x*xs-current_x*xs));
         if(cur_decel<=-(*hard_config).accel_x) acc_x=DECEL;}
      if(acc_y==NORMAL && dist_y*ys>current_y*ys+.0001)
        {cur_decel=((*hard_config).min_vel_y*(*hard_config).min_vel_y-cur_y_sp*cur_y_sp)/(2*(dist_y*ys-current_y*ys));
         if(cur_decel<=-(*hard_config).accel_y) acc_y=DECEL;}
      if(acc_z==NORMAL && dist_z*zs>current_z*zs+.0001)
        {cur_decel=((*hard_config).min_vel_z*(*hard_config).min_vel_z-cur_z_sp*cur_z_sp)/(2*(dist_z*zs-current_z*zs));
         if(cur_decel<=-(*hard_config).accel_z) acc_z=DECEL;}
      if(acc_a==NORMAL && dist_a*as>current_a*as+.0001)
        {cur_decel=((*hard_config).min_vel_a*(*hard_config).min_vel_a-cur_a_sp*cur_a_sp)/(2*(dist_a*as-current_a*as));
         if(cur_decel<=-(*hard_config).accel_a) acc_a=DECEL;} 
      
      //do acceleration routine and define speed
      if(acc_x==NORMAL && cur_x_sp<(*hard_config).vel_x && dist_x*xs>current_x*xs)
        {cur_x_sp=max(cur_x_sp+(*hard_config).accel_x*(*soft_config).dt,(*hard_config).min_vel_x);
         if(cur_x_sp>(*hard_config).vel_x) cur_x_sp=(*hard_config).vel_x;}
      if(acc_x==DECEL && cur_x_sp>(*hard_config).min_vel_x && dist_x*xs>current_x*xs)
        {cur_x_sp=cur_x_sp-(*hard_config).accel_x*(*soft_config).dt;
         if(cur_x_sp<(*hard_config).min_vel_x) cur_x_sp=(*hard_config).min_vel_x;}
      if(acc_y==NORMAL && cur_y_sp<(*hard_config).vel_y && dist_y*ys>current_y*ys)
        {cur_y_sp=max(cur_y_sp+(*hard_config).accel_y*(*soft_config).dt,(*hard_config).min_vel_y);
         if(cur_y_sp>(*hard_config).vel_y) cur_y_sp=(*hard_config).vel_y;}
      if(acc_y==DECEL && cur_y_sp>(*hard_config).min_vel_y  && dist_y*ys>current_y*ys)
        {cur_y_sp=cur_y_sp-(*hard_config).accel_y*(*soft_config).dt;
         if(cur_y_sp<(*hard_config).min_vel_y) cur_y_sp=(*hard_config).min_vel_y;}
      if(acc_z==NORMAL && cur_z_sp<(*hard_config).vel_z && dist_z*zs>current_z*zs)
        {cur_z_sp=max(cur_z_sp+(*hard_config).accel_z*(*soft_config).dt,(*hard_config).min_vel_z);
         if(cur_z_sp>(*hard_config).vel_z) cur_z_sp=(*hard_config).vel_z;}
      if(acc_z==DECEL && cur_z_sp>(*hard_config).min_vel_z && dist_z*zs>current_z*zs)
        {cur_z_sp=cur_z_sp-(*hard_config).accel_z*(*soft_config).dt;
         if(cur_z_sp<(*hard_config).min_vel_z) cur_z_sp=(*hard_config).min_vel_z;}
      if(acc_a==NORMAL && cur_a_sp<(*hard_config).vel_a && dist_a*as>current_a*as)
        {cur_a_sp=max(cur_a_sp+(*hard_config).accel_a*(*soft_config).dt,(*hard_config).min_vel_a);
         if(cur_a_sp>(*hard_config).vel_a) cur_a_sp=(*hard_config).vel_a;}
      if(acc_a==DECEL && cur_a_sp>(*hard_config).min_vel_a && dist_a*as>current_a*as)
         {cur_a_sp=cur_a_sp-(*hard_config).accel_a*(*soft_config).dt;
         if(cur_a_sp<(*hard_config).min_vel_a) cur_a_sp=(*hard_config).min_vel_a;}

      //increment position
      current_x=current_x+cur_x_sp*(*soft_config).dt*xs;
      current_y=current_y+cur_y_sp*(*soft_config).dt*ys;
      current_z=current_z+cur_z_sp*(*soft_config).dt*zs;
      current_a=current_a+cur_a_sp*(*soft_config).dt*as;
      inc_x=inc_x+cur_x_sp*(*soft_config).dt;
      inc_y=inc_y+cur_y_sp*(*soft_config).dt;
      inc_z=inc_z+cur_z_sp*(*soft_config).dt;
      inc_a=inc_a+cur_a_sp*(*soft_config).dt;
      if(dist_x*xs<=current_x*xs) {inc_x=inc_x-(current_x*xs-dist_x*xs); cur_x_sp=0; current_x=dist_x;}
      if(dist_y*ys<=current_y*ys) {inc_y=inc_y-(current_y*ys-dist_y*ys); cur_y_sp=0; current_y=dist_y;}
      if(dist_z*zs<=current_z*zs) {inc_z=inc_z-(current_z*zs-dist_z*zs); cur_z_sp=0; current_z=dist_z;}
      if(dist_a*as<=current_a*as) {inc_a=inc_a-(current_a*as-dist_a*xs); cur_a_sp=0; current_a=dist_a;}
      }
      
      //check to see if time to take a step
      if(inc_x>(*hard_config).x_per_step/2)
        {step_flag=ON;
         inc_x=inc_x-(*hard_config).x_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).x_sbit)==(*hard_config).x_sbit) step=step-(*hard_config).x_sbit;
            else if(x_step==LOW) step=step+(*hard_config).x_sbit;}
         else
           {if((step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit;
            else if(x_step==HIGH) step=step-(*hard_config).x_sbit;}
         if(x_step==HIGH) x_step=LOW; else x_step=HIGH;
        }
      if(inc_y>(*hard_config).y_per_step/2)
        {step_flag=ON;
         inc_y=inc_y-(*hard_config).y_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).y_sbit)==(*hard_config).y_sbit) step=step-(*hard_config).y_sbit;
            else if(y_step==LOW) step=step+(*hard_config).y_sbit;}
         else
           {if((step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit;
            else if(y_step==HIGH) step=step-(*hard_config).y_sbit;}
         if(y_step==HIGH) y_step=LOW; else y_step=HIGH;
        }
      if(inc_z>(*hard_config).z_per_step/2)
        {step_flag=ON;
         inc_z=inc_z-(*hard_config).z_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).z_sbit)==(*hard_config).z_sbit) step=step-(*hard_config).z_sbit;
            else if(z_step==LOW) step=step+(*hard_config).z_sbit;}
         else
           {if((step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit;
            else if(z_step==HIGH) step=step-(*hard_config).z_sbit;}
         if(z_step==HIGH) z_step=LOW; else z_step=HIGH;
        }
      if(inc_a>(*hard_config).a_per_step/2)
        {step_flag=ON;
         inc_a=inc_a-(*hard_config).a_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).a_sbit)==(*hard_config).a_sbit) step=step-(*hard_config).a_sbit;
            else if(a_step==LOW) step=step+(*hard_config).a_sbit;}
         else
           {if((step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit;
            else if(a_step==HIGH) step=step-(*hard_config).a_sbit;}
         if(a_step==HIGH) a_step=LOW; else a_step=HIGH;
        }
      if(step_flag)
        {step_flag=OFF;
         outportb((*hard_config).port, dir+step);
        }

      //check for completion of move
      if(dist_x*xs<=current_x*xs && dist_y*ys<=current_y*ys &&
         dist_z*zs<=current_z*zs && dist_a*as<=current_a*as)
        {cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0;
         if(*cur_speed<=min_speed && max_end_speed_sq<=min_speed*min_speed) *cur_speed=sqrt(max_end_speed_sq);
         break;
        }

      //check for keyboard speed control
      if((*soft_config).keyboard==ON && *sp_limit>0){
      if(key==0x21) {key=0; *sp_limit=*sp_limit+speed_inc; update_display(soft_config, 0, 0, 0, 0, 0, 0, *sp_limit, 6);}  //'F' key
      if(key==0x1f)
        {key=0; if(*sp_limit-speed_inc>0) *sp_limit=*sp_limit-speed_inc; update_display(soft_config, 0, 0, 0, 0, 0, 0, *sp_limit, 6);}} //'S' key

      //check for machine pause
      if(key==0x19) {key=0; msg=PAUSE;}

      if(*sp_limit>0){
      //check for io trip
      if((*soft_config).io1_cmm==ON && (c_port_read&(*hard_config).io1_bit)==LOW) {msg=IO1; break;}
      if((*soft_config).io2_cmm==ON && (c_port_read&(*hard_config).io2_bit)==LOW) {msg=IO2; break;}
      if((*soft_config).io3_cmm==ON && (c_port_read&(*hard_config).io3_bit)==LOW) {msg=IO3; break;}
      if((*soft_config).io4_cmm==ON && (c_port_read&(*hard_config).io4_bit)==LOW) {msg=IO4; break;}
      
      //check for keyboard trip (space bar or esc key)
      if((*soft_config).keyboard==ON && (key==0x39 || key==0x01)) {key=0; msg=BREAK; break;}

      //check for jog release
      if((*soft_config).keyboard==JOG && jogging==OFF) break;
      }
      if(*sp_limit<0 && break_flag==OFF){
      //check for io trip
      if((*soft_config).io1_cmm==ON && (c_port_read&(*hard_config).io1_bit)==LOW) msg=IO1;
      if((*soft_config).io2_cmm==ON && (c_port_read&(*hard_config).io2_bit)==LOW) msg=IO2;
      if((*soft_config).io3_cmm==ON && (c_port_read&(*hard_config).io3_bit)==LOW) msg=IO3;
      if((*soft_config).io4_cmm==ON && (c_port_read&(*hard_config).io4_bit)==LOW) msg=IO4;
      
      //check for keyboard trip on space bar or ESC
      if(key==0x39 || key==0x01) {key=0; msg=BREAK;}

      if(msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4)
        {break_flag=ON;
         if(msg!=BREAK) {(*current).pt_x=(*current).x_pos+current_x; (*current).pt_y=(*current).y_pos+current_y; 
                         (*current).pt_z=(*current).z_pos+current_z; (*current).pt_a=(*current).a_pos+current_a;}
         dist_x=current_x+sign(dist_x)*min(fabs(dist_x-current_x),cur_x_sp*cur_x_sp/(2*(*hard_config).accel_x));
         dist_y=current_y+sign(dist_y)*min(fabs(dist_y-current_y),cur_y_sp*cur_y_sp/(2*(*hard_config).accel_y));
         dist_z=current_z+sign(dist_z)*min(fabs(dist_z-current_z),cur_z_sp*cur_z_sp/(2*(*hard_config).accel_z));
         dist_a=current_a+sign(dist_a)*min(fabs(dist_a-current_a),cur_a_sp*cur_a_sp/(2*(*hard_config).accel_a));
        }}          

      if((*soft_config).update_after==OFF)
        {//update screen every .016 seconds
         display_count=display_count+(*soft_config).dt;
         if(display_count>.015)
           {display_count=0;
            if(*sp_limit>0){
            if(dist_x==0 && dist_y==0 && dist_z==0) speed=0;
            else speed=*cur_speed;
            update_display(soft_config, (*current).x_pos+current_x, (*current).y_pos+current_y,
                           (*current).z_pos+current_z, (*current).a_pos+current_a,
                           speed, (dist_a*as**cur_speed/total_vect), 0, display);
            }
            else {
            update_display(soft_config, (*current).x_pos+current_x, (*current).y_pos+current_y,
                           (*current).z_pos+current_z, (*current).a_pos+current_a,
                           sqrt(cur_x_sp*cur_x_sp+cur_y_sp*cur_y_sp+cur_z_sp*cur_z_sp),
                           cur_a_sp, 0, display);}
            display++; if(display==6) display=0;
           }
        }
     }

   if(msg==UNLIMIT) cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0;
   if((msg==COMPLETE || msg==PAUSE) && (*soft_config).keyboard!=JOG)
     {(*current).x_pos=(*current).x_pos+dist_x;
      (*current).y_pos=(*current).y_pos+dist_y;
      (*current).z_pos=(*current).z_pos+dist_z;
      (*current).a_pos=(*current).a_pos+dist_a;}
   else
     {(*current).x_pos=(*current).x_pos+current_x;
      (*current).y_pos=(*current).y_pos+current_y;
      (*current).z_pos=(*current).z_pos+current_z;
      (*current).a_pos=(*current).a_pos+current_a;}
   return msg;
}

int sign(float num)
{
   if(num<0) return -1;
   else return 1;
}

double total_vect(double x, double y, double z, double a)
{
  if(x==0 && y==0 && z==0) return fabs(a);
  return sqrt(x*x+y*y+z*z);
}

float get_max_accel(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{
   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).accel_x, fabs(y)/(*hard_config).accel_y,
                          fabs(z)/(*hard_config).accel_z, fabs(a)/(*hard_config).accel_a);
}

float get_max_speed(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{
   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).vel_x, fabs(y)/(*hard_config).vel_y,
                          fabs(z)/(*hard_config).vel_z, fabs(a)/(*hard_config).vel_a);
}

float get_min_speed(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{
   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).min_vel_x, fabs(y)/(*hard_config).min_vel_y,
                          fabs(z)/(*hard_config).min_vel_z, fabs(a)/(*hard_config).min_vel_a);
}

float get_max_4(float a, float b, float c, float d)
{
   if(a>=b&&a>=c&&a>=d) return a;
   if(b>=a&&b>=c&&b>=d) return b;
   if(c>=a&&c>=b&&c>=d) return c;
   return d;
}

int pause(struct hard_setup *hardware, struct soft_setup *software, float t, float *frate)
{
   float count=0;
   int msg=CONTINUE;
   BYTE s_port_read;

   init_key();
   while(count<t)
     {while(tick)
        {if(key==0x39 || key==0x01) {key=0; msg=BREAK; break;}
         if(key==0x21) {key=0; *frate=*frate+(*software).speed_inc; update_display(software, 0, 0, 0, 0, 0, 0, *frate, 6);}  //'F' key
         if(key==0x1f) {key=0; if(*frate-(*software).speed_inc>0) *frate=*frate-(*software).speed_inc; update_display(software, 0, 0, 0, 0, 0, 0, *frate, 6);} //'S' key
         if(key==0x19) {key=0; msg=PAUSE;}
        }
      if(msg==BREAK) break;
      tick=1;
      count=count+(*software).dt;
      if((*software).e_stop!=OFF) s_port_read=(248&inportb((*hardware).port+1))^128;
      if((*software).e_stop!=OFF && (*hardware).e_trip*(*hardware).e_bit-(s_port_read&(*hardware).e_bit)==0) {msg=E_STOP; break;}
     }
   restore_key();
   return msg;
}

unsigned long guage(struct soft_setup *software)
{
   float count=0;
   unsigned long i=0;

   while(count<1.0)
     {while(tick){sqrt(3); i++;}
      tick=1;
      count=count+(*software).dt;
     }
   i=i*.0083;
   if (i>100000) i=100000;
   return i;
}

//software interrupt functions
volatile int  tick_limit;            /*  The number of ticks before we call DOS ISR */
volatile int  tick_count;            /*  Current DOS tick counter                   */

void set_timer_freq(unsigned long freq)
{
   unsigned int new_timer_number ;
   _asm   cli              //disable interrupts

   if (freq<18) freq=18;
   tick_limit   = freq/18;         //number of ticks to give original
   tick_count   = 0;

    /*  Calculate the 'count down' value for the timer chip itself...  */
   new_timer_number=(freq==18) ? 0 : ((unsigned int) (1193180L/freq));
   /*  Re-program the timer chip...  */
   _asm    mov    dx, 43H
   _asm    mov    al, 34H
   _asm    out    dx, al
   _asm    mov    dx, 40H
   _asm    mov    ax, new_timer_number
   _asm    out    dx, al
   _asm    mov    al, ah
   _asm    out    dx, al

   _asm    sti       //enable interrupts
}

void (interrupt far *old_direct_timer)(void);

void _interrupt _far new_direct_timer(void)
{
   tick=0;
   if (--tick_count<=0)
     {tick_count=tick_limit;
      old_direct_timer();
     }
   else
   outp(0x20,0x20);
}

void begin_realtime(struct soft_setup *software, unsigned long freq)
{
   if(freq<18) freq=18;
   (*software).dt=1.0/freq;

   _asm   cli    //disable interrupts

   old_direct_timer  = _dos_getvect(0x08);
   set_timer_freq(freq);
   _dos_setvect(0x08,new_direct_timer);

   _asm   sti    //enable interrupts
}

void end_realtime(void)
{
   _asm   cli    //disable interrupts

   set_timer_freq(18);             /*  Ensures correct rate!  */

   _dos_setvect(0x08,old_direct_timer);

   _asm   sti    //enable interrupts
}

//keyboard interrupt functions
void (interrupt far *OldKeyboardRoutine)(void);

void _interrupt _far MyKeyboardRoutine(void)
{
   key = inportb(0x60);
   if(key==140 || key==141 || key==206 || key==202 || key==200 || 
      key==203 || key==205 || key==208 || key==201 || key==209) jogging=OFF;
   outportb(0x20, 0x20);  /* Issue EOI */
}

void init_key(void)
{
   _asm   cli    //disable interrupts

   OldKeyboardRoutine = _dos_getvect(9);
   _dos_setvect(0x09, MyKeyboardRoutine);
   key=0;

   _asm   sti    //enable interrupts
}

void restore_key(void)
{
   _asm   cli    //disable interrupts

   _dos_setvect(0x09, OldKeyboardRoutine);

   _asm   sti    //enable interrupts
}


//more high level user functions
int move(struct hard_setup *hard_config, struct soft_setup *soft_config, struct pos *current,
         float *cur_speed, float *sp_limit,
         double x2, double y2, double z2, double a2, double angle2, int object2,
         double x3, double y3, double z3, double a3, double angle3, int object3)
{
   double total_vect2, total_vect3, total_vect2_end, factor, total, temp;
   double radius, currentang, endang, incang, a_inc, z_inc, x_inc, y_inc;
   double x_start, y_start, z_start, a_start, x, y, z, a=0, slow_down_ang, i;
   double back_x_slow_down_ang, back_y_slow_down_ang, back_z_slow_down_ang, currentang_frac;
   float max_speed_2, max_accel, max_speed, high_speed=HIGH_SPEED;
   int msg, temp1, temp2, temp3, temp4, temp5;
   int acceleration=NORMAL, calcang=ON, break_flag=OFF;
   static int fd_fwd_msg=START_OVER, fd_fwd_msg_a=START_OVER;
   struct unit_vect vect2, vect3, vect2_end;
   static struct unit_vect vect1;
//   static int demo_inc;

//   if(version==DEMO)
//     {demo_inc++;
//      if(demo_inc>40) return QUIT;
//     }

   if(*sp_limit<0 && object2!=LINE_ENT && object2!=LINE_G0_ENT) return INVALID_PARAMETER;

   //hook keyboard interupt
   init_key();
   
   switch(object2)
     {case LINE_ENT:
         //get the max speed if HIGH_SPEED is selected
         if(*sp_limit<0) *sp_limit=get_max_speed(hard_config,x2,y2,z2,a2,total_vect(x2,y2,z2,a2));

         //get unit vector and total length for current object
         total_vect2=get_unit_vect(&vect2, x2, y2, z2, a2, angle2, object2);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) break;

         //save current backlash status
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         //get unit vector and total length for next object
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //see if backlash at end of move
         if(backlash_to_come(hard_config, &vect2, &vect3, fd_fwd_msg, fd_fwd_msg_a) || object3==LINE_G0_ENT) max_speed_2=0;
         else //get maximum ending speed squared
           {if(object3==LINE_ENT)
            max_speed_2=min(max_speed_sq(hard_config, x2, y2, z2, a2, total_vect2, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, x2, y2, z2, a2, total_vect2, &vect3);
           }

         //move machine accordingly
         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2,
                          x2, y2, z2, a2, total_vect2);

         //continue to stop if break or io trip
         if(msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4)
           {if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config, x2, y2, z2, a2, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                         x2*factor, y2*factor, z2*factor, a2*factor, total);
            (*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;

      case LINE_G0_ENT:
         //get backlash vector for current object
         vect2.x=x2; vect2.y=y2; vect2.z=z2; vect2.a=a2;

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) break;

         //save current backlash status
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         //get unit vector and total length for next object
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //set backlash to come vector
         backlash_to_come(hard_config, &vect2, &vect3, fd_fwd_msg, fd_fwd_msg_a); 
         
         //move machine accordingly
         msg=move_machine(hard_config, soft_config, current, cur_speed, &high_speed, 0,
                          x2, y2, z2, a2, total_vect2);
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;

      case XY_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(x2*x2+y2*y2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-y2,-x2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, -radius*cos(endang), -radius*sin(endang), z2, a2, angle2, XY_ARC_ENT);
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //see if backlash at end of move
         if(backlash_to_come(hard_config, &vect2_end, &vect3, START_OVER, START_OVER) || object3==LINE_G0_ENT) max_speed_2=0;
         else //get maximum ending speed squared
           {if(object3==LINE_ENT)
            max_speed_2=min(max_speed_sq(hard_config, vect2_end.x, vect2_end.y,
                            vect2_end.z, vect2_end.a, total_vect2_end, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, vect2_end.x, vect2_end.y, vect2_end.z,
                                          vect2_end.a, total_vect2_end, &vect3);
           }
         //max acceleration due to end vector
         max_accel=get_max_accel(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, vect2_end.a, total_vect2_end);

         //get loop increments
         if(angle2!=0) 
           {incang=2*atan(10*min((*hard_config).x_per_step, (*hard_config).y_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; z_inc=z2*incang/angle2;
           }
         else incang=1;

         //initialize ang begin angle loop
         x_start=(*current).x_pos;
         y_start=(*current).y_pos;
         z_start=(*current).z_pos;
         a_start=(*current).a_pos;
         for(i=0;i<labs(angle2/incang);i++)
           {currentang=currentang+incang;
            x=cos(currentang)*radius+x2-((*current).x_pos-x_start);
            y=sin(currentang)*radius+y2-((*current).y_pos-y_start);
            z=z_inc*(i+1)-((*current).z_pos-z_start);
            if(a2!=0) a=a_inc*(i+1)-((*current).a_pos-a_start);
            total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

            //do backlash
            msg=backlash(hard_config, soft_config, &vect1, &vect2);
            if(msg!=COMPLETE) goto DONE;

            //save backlash settings
            if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
            if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

            if(break_flag!=OFF) acceleration=DECEL;
            else acceleration=NORMAL;

            temp=*cur_speed**cur_speed*fabs(incang);
            //determine if backlash slow down angles have been exceeded
            if((*hard_config).back_x>.000009)
              {currentang_frac=fmod(currentang,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_x_slow_down_ang=temp/(2*(*hard_config).accel_y*total_vect2);
               if(back_x_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_y>.000009)
              {currentang_frac=fmod(currentang+PI/2,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_y_slow_down_ang=temp/(2*(*hard_config).accel_x*total_vect2);
               if(back_y_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            
            //determine if ending slow down angle has been exceeded
            if(calcang==ON) slow_down_ang=-(max_speed_2-*cur_speed**cur_speed)*fabs(incang)/(2*max_accel*total_vect2);
            if(slow_down_ang>=fabs(angle2-incang*(i+1))) acceleration=DECEL;
            
            if(acceleration==NORMAL)
              {max_speed=get_max_speed(hard_config, x, y, z, a, total_vect2);
               max_speed=max_speed*max_speed;
               msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed, x, y, z, a, total_vect2);
               calcang=ON;
              }

            if(acceleration==DECEL)
              {msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0, x, y, z, a, total_vect2);
               calcang=OFF;
              }

            if(msg!=COMPLETE)
              {if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
                 {break_flag=msg;
                  if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                                  (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
                  temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
                  temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
                  temp5=(*soft_config).io4_cmm;
                  (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
                  (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
                  //reset angle for stop
                  max_accel=get_max_accel(hard_config, x, y, z, a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  z2=z_inc*(i+1)+z2*slow_down_ang/fabs(angle2);
                  a2=a_inc*(i+1)+a2*slow_down_ang/fabs(angle2);
                  if(angle2<0) {angle2=incang*(i+1)-slow_down_ang; endang=currentang-slow_down_ang;}
                  else {angle2=incang*(i+1)+slow_down_ang; endang=currentang+slow_down_ang;}
                 }
               else goto DONE;
              }
           }
         x=cos(endang)*radius+x2-((*current).x_pos-x_start);
         y=sin(endang)*radius+y2-((*current).y_pos-y_start);
         z=z2-((*current).z_pos-z_start);
         if(a2!=0) a=a2-((*current).a_pos-a_start);
         total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) goto DONE;

         //save backlash settings
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2, x, y, z, a, total_vect2);
         if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
           {break_flag=msg;
            if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config, vect2.x, vect2.y, vect2.z, vect2.a, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                         x2*factor, y2*factor, z2*factor, a2*factor, total);
           }

         DONE :
         if(break_flag!=OFF)
           {(*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
            msg=break_flag;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;

      case YZ_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(y2*y2+z2*z2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-z2,-y2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, x2, -radius*cos(endang), -radius*sin(endang), a2, angle2, YZ_ARC_ENT);
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //see if backlash at end of move
         if(backlash_to_come(hard_config, &vect2_end, &vect3, START_OVER, START_OVER) || object3==LINE_G0_ENT) max_speed_2=0;
         else //get maximum ending speed squared
           {if(object3==LINE_ENT)
            max_speed_2=min(max_speed_sq(hard_config, vect2_end.x, vect2_end.y,
                            vect2_end.z, vect2_end.a, total_vect2_end, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, 
                                          vect2_end.a, total_vect2_end, &vect3);
           }
         //max acceleration due to end vector
         max_accel=get_max_accel(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, vect2_end.a, total_vect2_end);

         //get loop increments
         if(angle2!=0) 
           {incang=2*atan(10*min((*hard_config).y_per_step, (*hard_config).z_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; x_inc=x2*incang/angle2;
           }
         else incang=1;
         
         //initialize ang begin angle loop
         x_start=(*current).x_pos;
         y_start=(*current).y_pos;
         z_start=(*current).z_pos;
         a_start=(*current).a_pos;
         for(i=0;i<labs(angle2/incang);i++)
           {currentang=currentang+incang;
            y=cos(currentang)*radius+y2-((*current).y_pos-y_start);
            z=sin(currentang)*radius+z2-((*current).z_pos-z_start);
            x=x_inc*(i+1)-((*current).x_pos-x_start);
            if(a2!=0) a=a_inc*(i+1)-((*current).a_pos-a_start);
            total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

            //do backlash
            msg=backlash(hard_config, soft_config, &vect1, &vect2);
            if(msg!=COMPLETE) goto DONE;

            //save backlash settings
            if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
            if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

            if(break_flag!=OFF) acceleration=DECEL;
            else acceleration=NORMAL;
            
            temp=*cur_speed**cur_speed*fabs(incang);
            //determine if backlash slow down angles have been exceeded
            if((*hard_config).back_y>.000009)
              {currentang_frac=fmod(currentang,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_y_slow_down_ang=temp/(2*(*hard_config).accel_z*total_vect2);
               if(back_y_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_z>.000009)
              {currentang_frac=fmod(currentang+PI/2,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_z_slow_down_ang=temp/(2*(*hard_config).accel_y*total_vect2);
               if(back_z_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            
            //determine if ending slow down angle has been exceeded
            if(calcang==ON) slow_down_ang=-(max_speed_2-*cur_speed**cur_speed)*fabs(incang)/(2*max_accel*total_vect2);
            if(slow_down_ang>=fabs(angle2-incang*(i+1))) acceleration=DECEL;
            
            if(acceleration==NORMAL)
              {max_speed=get_max_speed(hard_config, x, y, z, a, total_vect2);
               max_speed=max_speed*max_speed;
               msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed, x, y, z, a, total_vect2);
               calcang=ON;
              }

            if(acceleration==DECEL)
              {msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0, x, y, z, a, total_vect2);
               calcang=OFF;
              }

            if(msg!=COMPLETE)
              {if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
                 {break_flag=msg;
                  if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                                  (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
                  temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
                  temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
                  temp5=(*soft_config).io4_cmm;
                  (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
                  (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
                  //reset angle for stop
                  max_accel=get_max_accel(hard_config, x, y, z, a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  x2=x_inc*(i+1)+x2*slow_down_ang/fabs(angle2);
                  a2=a_inc*(i+1)+a2*slow_down_ang/fabs(angle2);
                  if(angle2<0) {angle2=incang*(i+1)-slow_down_ang; endang=currentang-slow_down_ang;}
                  else {angle2=incang*(i+1)+slow_down_ang; endang=currentang+slow_down_ang;}
                 }
               else goto DONE2;
              }
           }
         y=cos(endang)*radius+y2-((*current).y_pos-y_start);
         z=sin(endang)*radius+z2-((*current).z_pos-z_start);
         x=x2-((*current).x_pos-x_start);
         if(a2!=0) a=a2-((*current).a_pos-a_start);
         total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) goto DONE2;

         //save backlash settings
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2, x, y, z, a, total_vect2);
         if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
           {break_flag=msg;
            if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config,vect2.x, vect2.y, vect2.z, vect2.a, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                         x2*factor, y2*factor, z2*factor, a2*factor, total);
           }

         DONE2 :
         if(break_flag!=OFF)
           {(*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
            msg=break_flag;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;

      case ZX_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(z2*z2+x2*x2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-x2,-z2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, -radius*sin(endang), y2, -radius*cos(endang), a2, angle2, ZX_ARC_ENT);
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //see if backlash at end of move
         if(backlash_to_come(hard_config, &vect2_end, &vect3, START_OVER, START_OVER) || object3==LINE_G0_ENT) max_speed_2=0;
         else //get maximum ending speed squared
           {if(object3==LINE_ENT)
            max_speed_2=min(max_speed_sq(hard_config, vect2_end.x, vect2_end.y,
                            vect2_end.z, vect2_end.a, total_vect2_end, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, 
                                          vect2_end.a, total_vect2_end, &vect3);
           }
         //max acceleration due to end vector
         max_accel=get_max_accel(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, vect2_end.a, total_vect2_end);

         //get loop increments
         if(angle2!=0) 
           {incang=2*atan(10*min((*hard_config).z_per_step, (*hard_config).x_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; y_inc=y2*incang/angle2;
           }
         else incang=1;
         
         //initialize ang begin angle loop
         x_start=(*current).x_pos;
         y_start=(*current).y_pos;
         z_start=(*current).z_pos;
         a_start=(*current).a_pos;
         for(i=0;i<labs(angle2/incang);i++)
           {currentang=currentang+incang;
            z=cos(currentang)*radius+z2-((*current).z_pos-z_start);
            x=sin(currentang)*radius+x2-((*current).x_pos-x_start);
            y=y_inc*(i+1)-((*current).y_pos-y_start);
            if(a2!=0) a=a_inc*(i+1)-((*current).a_pos-a_start);
            total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

            //do backlash
            msg=backlash(hard_config, soft_config, &vect1, &vect2);
            if(msg!=COMPLETE) goto DONE;

            //save backlash settings
            if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
            if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

            if(break_flag!=OFF) acceleration=DECEL;
            else acceleration=NORMAL;
            
            temp=*cur_speed**cur_speed*fabs(incang);
            //determine if backlash slow down angles have been exceeded
            if((*hard_config).back_z>.000009)
              {currentang_frac=fmod(currentang,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_z_slow_down_ang=temp/(2*(*hard_config).accel_x*total_vect2);
               if(back_z_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_x>.000009)
              {currentang_frac=fmod(currentang+PI/2,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_x_slow_down_ang=temp/(2*(*hard_config).accel_z*total_vect2);
               if(back_x_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            
            //determine if ending slow down angle has been exceeded
            if(calcang==ON) slow_down_ang=-(max_speed_2-*cur_speed**cur_speed)*fabs(incang)/(2*max_accel*total_vect2);
            if(slow_down_ang>=fabs(angle2-incang*(i+1))) acceleration=DECEL;
            
            if(acceleration==NORMAL)
              {max_speed=get_max_speed(hard_config, x, y, z, a, total_vect2);
               max_speed=max_speed*max_speed;
               msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed, x, y, z, a, total_vect2);
               calcang=ON;
              }

            if(acceleration==DECEL)
              {msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0, x, y, z, a, total_vect2);
               calcang=OFF;
              }

            if(msg!=COMPLETE)
              {if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
                 {break_flag=msg;
                  if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                                  (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
                  temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
                  temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
                  temp5=(*soft_config).io4_cmm;
                  (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
                  (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
                  //reset angle for stop
                  max_accel=get_max_accel(hard_config, vect2.x, vect2.y, vect2.z, vect2.a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  y2=y_inc*(i+1)+y2*slow_down_ang/fabs(angle2);
                  a2=a_inc*(i+1)+a2*slow_down_ang/fabs(angle2);
                  if(angle2<0) {angle2=incang*(i+1)-slow_down_ang; endang=currentang-slow_down_ang;}
                  else {angle2=incang*(i+1)+slow_down_ang; endang=currentang+slow_down_ang;}
                 }
               else goto DONE3;
              }
           }
         z=cos(endang)*radius+z2-((*current).z_pos-z_start);
         x=sin(endang)*radius+x2-((*current).x_pos-x_start);
         y=y2-((*current).y_pos-y_start);
         if(a2!=0) a=a2-((*current).a_pos-a_start);
         total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) goto DONE3;

         //save backlash settings
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2, x, y, z, a, total_vect2);
         if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
           {break_flag=msg;
            if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config, x, y, z, a, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                             x2*factor, y2*factor, z2*factor, a2*factor, total);
           }

         DONE3 :
         if(break_flag!=OFF)
           {(*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
            msg=break_flag;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;
     }

   //reset backlash to come messages accordingly
   if((x3==0 && y3==0 && z3==0) || msg!=COMPLETE || msg!=PAUSE) fd_fwd_msg=START_OVER;
   else fd_fwd_msg=CONTINUE;
   if(a3==0 || msg!=COMPLETE || msg!=PAUSE) fd_fwd_msg_a=START_OVER;
   else fd_fwd_msg_a=CONTINUE;

   //restore keyboard interupt
   restore_key();
   return msg;
}

//backlash function
int backlash(struct hard_setup *hard_config, struct soft_setup *soft_config,
             struct unit_vect *vect1, struct unit_vect *vect2)
{
   double x=0, y=0, z=0, a=0, total;
   float max_speed, cur_speed=0;
   int msg, temp1, temp2, temp3, temp4, temp5, temp6, temp7, temp8, temp9, temp10, temp11, temp12;
   struct pos pt;
   pt.x_pos=pt.y_pos=pt.z_pos=pt.a_pos=0;

   //determine if backlash takes place
   if(((*vect1).x*(*vect2).x>=0 || (*hard_config).back_x<=.000009) &&
      ((*vect1).y*(*vect2).y>=0 || (*hard_config).back_y<=.000009) &&
      ((*vect1).z*(*vect2).z>=0 || (*hard_config).back_z<=.000009) &&
      ((*vect1).a*(*vect2).a>=0 || (*hard_config).back_a<=.000009))
      return COMPLETE;

   //determine backlash
   if((*vect1).x*(*vect2).x<0 && (*hard_config).back_x>.000009)
     {if((*vect2).x<0) x=-(*hard_config).back_x; else x=(*hard_config).back_x;}
   if((*vect1).y*(*vect2).y<0 && (*hard_config).back_y>.000009)
     {if((*vect2).y<0) y=-(*hard_config).back_y; else y=(*hard_config).back_y;}
   if((*vect1).z*(*vect2).z<0 && (*hard_config).back_z>.000009)
     {if((*vect2).z<0) z=-(*hard_config).back_z; else z=(*hard_config).back_z;}
   if((*vect1).a*(*vect2).a<0 && (*hard_config).back_a>.000009)
     {if((*vect2).a<0) a=-(*hard_config).back_a; else a=(*hard_config).back_a;}

   //determine total backlash vector
   total=total_vect(x, y, z, a);

   //determine max speed
   max_speed=get_max_speed(hard_config, x, y, z, a, total);

   //do backlash compensation
   temp1=(*soft_config).keyboard;
   temp2=(*soft_config).io1_cmm;
   temp3=(*soft_config).io2_cmm;
   temp4=(*soft_config).io3_cmm;
   temp5=(*soft_config).io4_cmm;
   temp6=(*soft_config).display_x;
   temp7=(*soft_config).display_y;
   temp8=(*soft_config).display_z;
   temp9=(*soft_config).display_s;
   temp10=(*soft_config).display_as;
   temp11=(*soft_config).display_a;
   temp12=(*soft_config).display_fr;
   (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
   (*soft_config).io3_cmm=(*soft_config).io4_cmm=
   (*soft_config).display_x=(*soft_config).display_y=(*soft_config).display_z=(*soft_config).display_a=
   (*soft_config).display_s=(*soft_config).display_as=(*soft_config).display_fr=OFF;
   msg=move_machine(hard_config, soft_config, &pt, &cur_speed, &max_speed, 0, x, y, z, a, total);
   (*soft_config).keyboard=temp1;
   (*soft_config).io1_cmm=temp2;
   (*soft_config).io2_cmm=temp3;
   (*soft_config).io3_cmm=temp4;
   (*soft_config).io4_cmm=temp5;
   (*soft_config).display_x=temp6;
   (*soft_config).display_y=temp7;
   (*soft_config).display_z=temp8;
   (*soft_config).display_s=temp9;
   (*soft_config).display_as=temp10;
   (*soft_config).display_a=temp11;
   (*soft_config).display_fr=temp12;
   return msg;
}

double get_unit_vect(struct unit_vect *vect, double x, double y, double z, double a, double angle, int object)
{
   double total, temp;

   if(object==XY_ARC_ENT)
     {temp=x; x=y; y=-temp;
      if(angle<0) {x=-x; y=-y;}
      if(angle!=0)
        {z=z/fabs(angle);
         a=a/fabs(angle);
        }
     }
   if(object==YZ_ARC_ENT)
     {temp=y; y=z; z=-temp;
      if(angle<0) {y=-y; z=-z;}
      if(angle!=0)
        {x=x/fabs(angle);
         a=a/fabs(angle);
        }
     }
   if(object==ZX_ARC_ENT)
     {temp=z; z=x; x=-temp;
      if(angle<0) {z=-z; x=-x;}
      if(angle!=0)
        {y=y/fabs(angle);
         a=a/fabs(angle);
        }
     }
   total=total_vect(x, y, z, a);
   if(total==0) {(*vect).x=(*vect).y=(*vect).z=(*vect).a=0; return total;}
   (*vect).x=x/total;
   (*vect).y=y/total;
   (*vect).z=z/total;
   (*vect).a=a/total;
   if(object!=LINE_ENT) total=1;
   return total;
}

//find maximum ending speed squared
float max_speed_sq(struct hard_setup *hard_config, 
                   float x1, float y1, float z1, float a1, float total1,
                   struct unit_vect *vect2)
{
   float vx2, vy2, vz2, va2, den;
   float max_speed;

   if(total1==0) return 0;

   //define maximum speed squared
   max_speed=get_max_speed(hard_config, x1, y1, z1, a1, total1);
   max_speed=max_speed*max_speed;

   //determine maximum speed squared entering new vector
   if((den=fabs((*vect2).x-x1/total1)*(fabs((*vect2).x)+fabs(x1/total1)))<=.00000001) vx2=max_speed;
   else vx2=(*hard_config).accel_x*(*hard_config).x_per_step/den;
   if((den=fabs((*vect2).y-y1/total1)*(fabs((*vect2).y)+fabs(y1/total1)))<=.00000001) vy2=max_speed;
   else vy2=(*hard_config).accel_y*(*hard_config).y_per_step/den;
   if((den=fabs((*vect2).z-z1/total1)*(fabs((*vect2).z)+fabs(z1/total1)))<=.00000001) vz2=max_speed;
   else vz2=(*hard_config).accel_z*(*hard_config).z_per_step/den;
   if((den=fabs((*vect2).a-a1/total1)*(fabs((*vect2).a)+fabs(a1/total1)))<=.00000001) va2=max_speed;
   else va2=(*hard_config).accel_a*(*hard_config).a_per_step/den;

   //return lowest speed
   if(vx2<=vy2 && vx2<=vz2 && vx2<va2 && vx2<max_speed) return vx2;
   if(vy2<=vx2 && vy2<=vz2 && vy2<va2 && vy2<max_speed) return vy2;
   if(vz2<=vx2 && vz2<=vy2 && vz2<va2 && vz2<max_speed) return vz2;
   if(va2<=vx2 && va2<=vy2 && va2<vz2 && va2<max_speed) return va2;
   return max_speed;
}

//get maximum ending speed due to length of next line
float max_speed_sq_line(struct hard_setup *hard_config, double x, double y, double z, double a, double total)
{
   float max_accel, min_speed;

   if(total==0) return 0;
   max_accel=get_max_accel(hard_config, x, y, z, a, total);
   min_speed=get_min_speed(hard_config, x, y, z, a, total);
   return total*2*max_accel+min_speed*min_speed;
}

//check for backlash to come in next move
int backlash_to_come(struct hard_setup *hard_config, struct unit_vect *vect1, struct unit_vect *vect2,
                     int fd_fwd_msg, int fd_fwd_msg_a)
{
   static struct unit_vect prev;
   int msg=FALSE;

   if(fd_fwd_msg==START_OVER) {prev.x=(*vect1).x; prev.y=(*vect1).y; prev.z=(*vect1).z;}
   if(fd_fwd_msg_a==START_OVER) prev.a=(*vect1).a;

   //check for backlash to come
   if((prev.x*(*vect2).x<0 && (*hard_config).back_x>.000009) ||
      (prev.y*(*vect2).y<0 && (*hard_config).back_y>.000009) ||
      (prev.z*(*vect2).z<0 && (*hard_config).back_z>.000009) ||
      (prev.a*(*vect2).a<0 && (*hard_config).back_a>.000009))
      msg=TRUE;

   if((*vect2).x!=0) prev.x=(*vect2).x; if((*vect2).y!=0) prev.y=(*vect2).y;
   if((*vect2).z!=0) prev.z=(*vect2).z; if((*vect2).a!=0) prev.a=(*vect2).a;
   return msg;
}

int jog(struct hard_setup *hardware, struct soft_setup *software, struct pos *pt, int type, double inc, int axis, int dir)
{
   float j_speed=0, cur_speed=0;
   double inc_x, inc_y, inc_z, inc_a;
   int x=0, y=0, z=0, a=0, msg=COMPLETE;

   if(axis==X_AXIS) x=dir; if(axis==Y_AXIS) y=dir; if(axis==Z_AXIS) z=dir; if(axis==A_AXIS) a=dir; 
   if(type==FAST || type==INC)
     {if(axis==X_AXIS) j_speed=(*software).fjog_x; if(axis==Y_AXIS) j_speed=(*software).fjog_y;
      if(axis==Z_AXIS) j_speed=(*software).fjog_z; if(axis==A_AXIS) j_speed=(*software).fjog_a;}
   else 
     {if(axis==X_AXIS) j_speed=(*software).sjog_x; if(axis==Y_AXIS) j_speed=(*software).sjog_y;
      if(axis==Z_AXIS) j_speed=(*software).sjog_z; if(axis==A_AXIS) j_speed=(*software).sjog_a;}
   if(type==INC) 
     {if(fabs(inc)<.000001) inc=0;
      inc_x=inc_y=inc_z=inc_a=inc;
      msg=move(hardware, software, pt, &cur_speed, &j_speed,
               x*inc_x, y*inc_y, z*inc_z, a*inc_a, NULL, LINE_ENT, 0, 0, 0, 0, NULL, LINE_ENT);
     }
   else
     {(*software).keyboard=JOG; jogging=ON;
      msg=move(hardware, software, pt, &cur_speed, &j_speed,
               x*9999, y*9999, z*9999, a*9999,
               NULL, LINE_ENT, 0, 0, 0, 0, NULL, LINE_ENT);
      (*software).keyboard=ON;
      if(type==FAST && msg==COMPLETE) 
         msg=move(hardware, software, pt, &cur_speed, &j_speed,
                  x*cur_speed*cur_speed/(2*(*hardware).accel_x), y*cur_speed*cur_speed/(2*(*hardware).accel_y),
                  z*cur_speed*cur_speed/(2*(*hardware).accel_z), a*cur_speed*cur_speed/(2*(*hardware).accel_a),
                  NULL, LINE_ENT, 0, 0, 0, 0, NULL, LINE_ENT);
     }
   return msg;
}

int homing(struct hard_setup *hardware, struct soft_setup *software, struct pos *pt, 
           int id1, int dir1, int id2, int dir2)
{
   int i, msg=COMPLETE, x=0, y=0, z=0, a=0, temp_a_follow;
   float old_x_sp, old_y_sp, old_z_sp, old_a_sp, cur_speed=0, sp_limit=0;

   old_x_sp=(*hardware).vel_x; old_y_sp=(*hardware).vel_y;
   old_z_sp=(*hardware).vel_z; old_a_sp=(*hardware).vel_a;
   temp_a_follow=(*software).a_follow;

   (*hardware).vel_x=(*hardware).h_spd_x; (*hardware).vel_y=(*hardware).h_spd_y;
   (*hardware).vel_z=(*hardware).h_spd_z; (*hardware).vel_a=(*hardware).h_spd_a;
   (*software).a_follow=INDEPENDANT;

   if(id1==X_AXIS) {if(dir1==NULL) x=(*software).home_x; else x=dir1;}
   if(id1==Y_AXIS) {if(dir1==NULL) y=(*software).home_y; else y=dir1;}
   if(id1==Z_AXIS) {if(dir1==NULL) z=(*software).home_z; else z=dir1;}
   if(id1==A_AXIS && (*software).a_status==ON) {if(dir1==NULL) a=(*software).home_a; else a=dir1;}
   if(id2==X_AXIS) {if(dir2==NULL) x=(*software).home_x; else x=dir2;}
   if(id2==Y_AXIS) {if(dir2==NULL) y=(*software).home_y; else y=dir2;}
   if(id2==Z_AXIS) {if(dir2==NULL) z=(*software).home_z; else z=dir2;}
   if(id2==A_AXIS && (*software).a_status==ON) {if(dir2==NULL) a=(*software).home_a; else a=dir2;}
   
   for(i=0; i<4; i++)
     {msg=move(hardware, software, pt, &cur_speed, &sp_limit,
               x*9999, y*9999, z*9999, a*9999,
               NULL, LINE_G0_ENT, 0, 0, 0, 0, NULL, LINE_ENT);
      if(msg==X_PLIMIT || msg==X_NLIMIT) x=0; if(msg==Y_PLIMIT || msg==Y_NLIMIT) y=0;
      if(msg==Z_PLIMIT || msg==Z_NLIMIT) z=0; if(msg==A_PLIMIT || msg==A_NLIMIT) a=0;
      if(msg==BREAK || msg==E_STOP || msg==QUIT) goto DONE;
     }

   for(i=0; i<4; i++)
     {cur_speed=sp_limit=x=y=z=a=0;
      if(id1==X_AXIS && i==0) {if(dir1==NULL) x=-(*software).home_x; else x=-dir1; sp_limit=(*hardware).min_vel_x;}
      if(id2==X_AXIS && i==0) {if(dir2==NULL) x=-(*software).home_x; else x=-dir2; sp_limit=(*hardware).min_vel_x;}
      if(id1==Y_AXIS && i==1) {if(dir1==NULL) y=-(*software).home_y; else y=-dir1; sp_limit=(*hardware).min_vel_y;}
      if(id2==Y_AXIS && i==1) {if(dir2==NULL) y=-(*software).home_y; else y=-dir2; sp_limit=(*hardware).min_vel_y;}
      if(id1==Z_AXIS && i==2) {if(dir1==NULL) z=-(*software).home_z; else z=-dir1; sp_limit=(*hardware).min_vel_z;}
      if(id2==Z_AXIS && i==2) {if(dir2==NULL) z=-(*software).home_z; else z=-dir2; sp_limit=(*hardware).min_vel_z;}
      if(id1==A_AXIS && (*software).a_status==ON && i==3)
        {if(dir1==NULL) a=-(*software).home_a; else a=-dir1; sp_limit=(*hardware).min_vel_a;}
      if(id2==A_AXIS && (*software).a_status==ON && i==3)
        {if(dir2==NULL) a=-(*software).home_a; else a=-dir2; sp_limit=(*hardware).min_vel_a;}
      msg=move(hardware, software, pt, &cur_speed, &sp_limit,
               x*9999, y*9999, z*9999, a*9999,
               NULL, LINE_ENT, 0, 0, 0, 0, NULL, LINE_ENT);
      if(msg==BREAK || msg==E_STOP || msg==QUIT) goto DONE;
      if(x!=0) (*pt).x_pos=0;
      if(y!=0) (*pt).y_pos=0;
      if(z!=0) (*pt).z_pos=0;
      if(a!=0) (*pt).a_pos=0;
     }
   msg=COMPLETE;

   DONE:
   (*hardware).vel_x=old_x_sp; (*hardware).vel_y=old_y_sp;
   (*hardware).vel_z=old_z_sp; (*hardware).vel_a=old_a_sp;
   (*software).a_follow=temp_a_follow;
   return msg;
}

